# -*- encoding:UTF-8 -*-
from naoqi import ALProxy
import time
import cmath

IP = "192.168.0.106"
PORT = 9559

if __name__ == '__main__':
    motionProxy = ALProxy("ALMotion", IP, PORT)
    ttsProxy = ALProxy("ALTextToSpeech", IP, PORT)
    motionProxy.wakeUp()
    # names = 'Body'
    # motionProxy.stiffnessInterpolation(names, 1.0, 1.0)
    # motionProxy.wakeUp()
    # proxy.setAngles("RShoulderPitch", -50 * cmath.pi / 180, 0.2)
    # proxy.setAngles("LShoulderPitch", -50 * cmath.pi / 180, 0.2)
    #
    # proxy.setAngles("RShoulderRoll", -10 * cmath.pi / 180, 0.2)
    # proxy.setAngles("LShoulderRoll", 10 * cmath.pi / 180, 0.2)
    #
    # proxy.setAngles("RElbowRoll", 45 * cmath.pi / 180, 0.2)
    # proxy.setAngles("LElbowRoll", -45 * cmath.pi / 180, 0.2)
    #
    # proxy.setAngles("RElbowYaw", 45 * cmath.pi / 180, 0.2)
    # proxy.setAngles("LElbowYaw", -45 * cmath.pi / 180, 0.2)
    #
    # proxy.setAngles("RHand", 0.4, 0.2)
    # proxy.setAngles("LHand", 0.1, 0.2)

    # 抓取小球前
    ttsProxy.say("I will raise my hands")
    motionProxy.setAngles("RShoulderPitch", -52.0 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LShoulderPitch", -52.0 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RHand", 1.0, 0.2)
    motionProxy.setAngles("LHand", 1.0, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RWristYaw", 12.7 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LWristYaw", 12.7 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RElbowYaw", 73.3 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LElbowYaw", -73.3 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RElbowRoll", 20 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LElbowRoll", -20 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RShoulderRoll", -7.5 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LShoulderRoll", 7.5 * cmath.pi / 180, 0.2)
    time.sleep(5)




    # 抓取小球时
    ttsProxy.say("I will catch the boll")
    leftArmEnable = False
    rightArmEnable = False
    motionProxy.setMoveArmsEnabled(leftArmEnable, rightArmEnable)
    motionProxy.moveTo(0.1, 0.0, 0.0)
    motionProxy.setMoveArmsEnabled(True, True)
    motionProxy.setAngles("RShoulderPitch", -8.1 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LShoulderPitch", -8.1 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RElbowRoll", 42.5 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LElbowRoll", -42.5 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RWristYaw", 66.1 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LWristYaw", -66.1 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RElbowYaw", 13.7 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LElbowYaw", -13.7 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RShoulderRoll", 1.1* cmath.pi / 180, 0.2)
    motionProxy.setAngles("LShoulderRoll", -1.1 * cmath.pi / 180, 0.2)
    time.sleep(3)

    motionProxy.setAngles("RHand", 0.2, 0.2)
    motionProxy.setAngles("LHand", 0.2, 0.2)
    time.sleep(1)
    # 抓取小球后的
    motionProxy.setAngles("RElbowYaw", 37.3 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LElbowYaw", -37.3 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RElbowRoll", 87.5 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LElbowRoll", -87.5 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RShoulderPitch", 48.9 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LShoulderPitch", 48.9 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RShoulderRoll", -25.3 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LShoulderRoll", 25.3 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RWristYaw", 33.2 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LWristYaw", -33.2 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RHand", 0.63, 0.2)
    motionProxy.setAngles("LHand", 0.63, 0.2)
    time.sleep(1)
